20. Sigma Point Prediction Assignment 1

Assignment Sigma Point Prediction

Sigma Point Prediction

Task Description:

You will be completing the missing code in ukf.cpp, SigmaPointPrediction() function.

Task List:

Task Feedback:

Nice! Have you tested your code below?

Helpful Equations

x = \begin{bmatrix} p_x \\ p_y \\ v \\ \psi \\ \dot{\psi} \end{bmatrix}
If \large \dot{\psi}_k is not zero:
\text{State}= x_{k+1} = x_k + \begin{bmatrix} \frac{v_k}{\dot{\psi}_k}(sin(\psi_k+\dot{\psi}_k\Delta t )-sin(\psi_k))\\ \frac{v_k}{\dot{\psi}_k}(-cos(\psi_k+\dot{\psi}_k\Delta t )+cos(\psi_k))\\ 0\\ \dot{\psi}_k\Delta t\\ 0 \end{bmatrix} +\begin{bmatrix} \frac{1}{2}(\Delta t)^2cos(\psi_k)\nu_{a,k}\\ \frac{1}{2}(\Delta t)^2sin(\psi_k)\nu_{a,k}\\ \Delta t\cdot\nu_{a,k}\\ \frac{1}{2}(\Delta t)^2\nu_{\ddot{\psi},k}\\ \Delta t\cdot\nu_{\ddot{\psi},k} \end{bmatrix}
If \large \dot{\psi}_k is zero:
\text{State} =x_{k+1} = x_k + \begin{bmatrix} v_kcos(\psi_k)\Delta t\\ v_ksin(\psi_k)\Delta t\\ 0\\ \dot{\psi}_k\Delta t\\ 0 \end{bmatrix} +\begin{bmatrix} \frac{1}{2}(\Delta t)^2cos(\psi_k)\nu_{a,k}\\ \frac{1}{2}(\Delta t)^2sin(\psi_k)\nu_{a,k}\\ \Delta t\cdot\nu_{a,k}\\ \frac{1}{2}(\Delta t)^2\nu_{\ddot{\psi},k}\\ \Delta t\cdot\nu_{\ddot{\psi},k} \end{bmatrix}

Notice that when \large \dot{\psi_k} = 0,

the term \large \dot{\psi_k}\Delta{t} would also equal zero.

Start Quiz: 

#include "Dense"
#include "ukf.h"

using Eigen::MatrixXd;

int main() {

  // Create a UKF instance
  UKF ukf;

  /**
   * Programming assignment calls
   */
  MatrixXd Xsig_pred = MatrixXd(15, 5);
  ukf.SigmaPointPrediction(&Xsig_pred);

  return 0;
}
#include <iostream>
#include "ukf.h"

using Eigen::MatrixXd;
using Eigen::VectorXd;

UKF::UKF() {
  Init();
}

UKF::~UKF() {

}

void UKF::Init() {

}


/**
 * Programming assignment functions: 
 */

void UKF::SigmaPointPrediction(MatrixXd* Xsig_out) {

  // set state dimension
  int n_x = 5;

  // set augmented dimension
  int n_aug = 7;

  // create example sigma point matrix
  MatrixXd Xsig_aug = MatrixXd(n_aug, 2 * n_aug + 1);
  Xsig_aug <<
    5.7441,  5.85768,   5.7441,   5.7441,   5.7441,   5.7441,   5.7441,   5.7441,   5.63052,   5.7441,   5.7441,   5.7441,   5.7441,   5.7441,   5.7441,
      1.38,  1.34566,  1.52806,     1.38,     1.38,     1.38,     1.38,     1.38,   1.41434,  1.23194,     1.38,     1.38,     1.38,     1.38,     1.38,
    2.2049,  2.28414,  2.24557,  2.29582,   2.2049,   2.2049,   2.2049,   2.2049,   2.12566,  2.16423,  2.11398,   2.2049,   2.2049,   2.2049,   2.2049,
    0.5015,  0.44339, 0.631886, 0.516923, 0.595227,   0.5015,   0.5015,   0.5015,   0.55961, 0.371114, 0.486077, 0.407773,   0.5015,   0.5015,   0.5015,
    0.3528, 0.299973, 0.462123, 0.376339,  0.48417, 0.418721,   0.3528,   0.3528,  0.405627, 0.243477, 0.329261,  0.22143, 0.286879,   0.3528,   0.3528,
         0,        0,        0,        0,        0,        0,  0.34641,        0,         0,        0,        0,        0,        0, -0.34641,        0,
         0,        0,        0,        0,        0,        0,        0,  0.34641,         0,        0,        0,        0,        0,        0, -0.34641;

  // create matrix with predicted sigma points as columns
  MatrixXd Xsig_pred = MatrixXd(n_x, 2 * n_aug + 1);

  double delta_t = 0.1; // time diff in sec

  /**
   * Student part begin
   */

  // predict sigma points

  // avoid division by zero

  // write predicted sigma points into right column

  /**
   * Student part end
   */

  // print result
  std::cout << "Xsig_pred = " << std::endl << Xsig_pred << std::endl;

  // write result
  *Xsig_out = Xsig_pred;
}
#ifndef UKF_H
#define UKF_H

#include "Dense"

class UKF {
 public:
  /**
   * Constructor
   */
  UKF();

  /**
   * Destructor
   */
  virtual ~UKF();

  /**
   * Init Initializes Unscented Kalman filter
   */
  void Init();

  /**
   * Student assignment functions
   */
  void GenerateSigmaPoints(Eigen::MatrixXd* Xsig_out);
  void AugmentedSigmaPoints(Eigen::MatrixXd* Xsig_out);
  void SigmaPointPrediction(Eigen::MatrixXd* Xsig_out);
  void PredictMeanAndCovariance(Eigen::VectorXd* x_pred, 
                                Eigen::MatrixXd* P_pred);
  void PredictRadarMeasurement(Eigen::VectorXd* z_out, 
                               Eigen::MatrixXd* S_out);
  void UpdateState(Eigen::VectorXd* x_out, 
                   Eigen::MatrixXd* P_out);
};

#endif  // UKF_H

expected result:

Xsig_pred =

5.93553 6.06251 5.92217 5.9415 5.92361 5.93516 5.93705 5.93553 5.80832 5.94481 5.92935 5.94553 5.93589 5.93401 5.93553

1.48939 1.44673 1.66484 1.49719 1.508 1.49001 1.49022 1.48939 1.5308 1.31287 1.48182 1.46967 1.48876 1.48855 1.48939

2.2049 2.28414 2.24557 2.29582 2.2049 2.2049 2.23954 2.2049 2.12566 2.16423 2.11398 2.2049 2.2049 2.17026 2.2049

0.53678 0.473387 0.678098 0.554557 0.643644 0.543372 0.53678 0.538512 0.600173 0.395462 0.519003 0.429916 0.530188 0.53678 0.535048

0.3528 0.299973 0.462123 0.376339 0.48417 0.418721 0.3528 0.387441 0.405627 0.243477 0.329261 0.22143 0.286879 0.3528 0.318159

Next Concept